﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using System.IO;

namespace SkyBot
{
    public partial class SkyBotForm : Form
    {
        bool Connection = false;

        BufferedGraphicsContext currentContext;
        BufferedGraphics AccelBuffer;
        BufferedGraphics GyroBuffer;
        BufferedGraphics MagBuffer;
        BufferedGraphics MagXYBuffer;
        BufferedGraphics MagXZBuffer;
        BufferedGraphics MagYZBuffer;
        BufferedGraphics CompassBuffer;
        //double MotorRange = 0.5;

        //double GyroXSensitivityRange = 0.5;
        //double GyroYSensitivityRange = 0.5;
        //double GyroZSensitivityRange = 0.5;

        double RollRange = 1.0;
        double PitchRange = 1.0;
        double YawRange = 1.0;
        double ThrottleRange = 0.4;

        List<int> GraphicAccelX;
        List<int> GraphicAccelY;
        List<int> GraphicAccelZ;

        List<int> GraphicGyroX;
        List<int> GraphicGyroY;
        List<int> GraphicGyroZ;

        List<int> GraphicMagX;
        List<int> GraphicMagY;
        List<int> GraphicMagZ;

        List<int> GraphicPIDX;
        List<int> GraphicPIDY;
        List<int> GraphicPIDZ;

        const int GraphicWidth = 200;

        float x1, x2, y1, y2, z1, z2;
        float minx, maxx, miny, maxy, minz, maxz;
        int ParamX, ParamY, ParamZ;

        double North = 0.0;
        //float x1 = 0.0f, x2 = 0.0f, y1 = 0.0f, y2 = 0.0f, z1 = 0.0f, z21 = 0.0f;
        //float minx, maxx, miny, maxy, minz, maxz;
        //float pointX, pointY, pointZ;

        bool Calibration = false;

        //StreamWriter Writer;

        Random rnd;

        //double Forward = 0.0;
        //double Right = 0.0;
        //double Rotation = 0.0;

        //int Roll = 4000;
        //int Pitch = 4000;
        //int Yaw = 127;
        //int Throttle = 0;

        //int RollP;
        //int RollI;
        //int RollD;
        //int PitchP;
        //int PitchI;
        //int PitchD;
        //int YawP;
        //int YawI;
        //int YawD;

        //int RollCenter = 4000;//115 3640
        //int PitchCenter = 4000;//117 3620
        //int YawCenter = 4000;//114 3750

        //int Sensitivity = 127;

        bool Start = false;

        //bool Calibrate = false;

        //bool MouseDown = false;

        /*bool KeyDownRollUp = false;
        bool KeyDownRollDown = false;
        bool KeyDownPitchUp= false;
        bool KeyDownPitchDown = false;
        bool KeyDownYawUp = false;
        bool KeyDownYawDown = false;
        bool KeyDownThrottleUp = false;
        bool KeyDownThrottleDown = false;*/

        public SkyBotForm()
        {
            InitializeComponent();

            rnd = new Random();

            //Writer = new StreamWriter("calibration.txt");

            currentContext = BufferedGraphicsManager.Current;

            AccelBuffer = currentContext.Allocate(AccelPanel1.CreateGraphics(), AccelPanel1.DisplayRectangle);
            GyroBuffer = currentContext.Allocate(GyroPanel1.CreateGraphics(), GyroPanel1.DisplayRectangle);
            MagBuffer = currentContext.Allocate(MagPanel1.CreateGraphics(), MagPanel1.DisplayRectangle);

            MagXYBuffer = currentContext.Allocate(CompassPanelXY.CreateGraphics(), CompassPanelXY.DisplayRectangle);
            MagXZBuffer = currentContext.Allocate(CompassPanelXZ.CreateGraphics(), CompassPanelXZ.DisplayRectangle);
            MagYZBuffer = currentContext.Allocate(CompassPanelYZ.CreateGraphics(), CompassPanelYZ.DisplayRectangle);

            CompassBuffer = currentContext.Allocate(CompassPanel.CreateGraphics(), CompassPanel.DisplayRectangle);

            GraphicAccelX = new List<int>();
            GraphicAccelY = new List<int>();
            GraphicAccelZ = new List<int>();

            GraphicGyroX = new List<int>();
            GraphicGyroY = new List<int>();
            GraphicGyroZ = new List<int>();

            GraphicMagX = new List<int>();
            GraphicMagY = new List<int>();
            GraphicMagZ = new List<int>();

            GraphicPIDX = new List<int>();
            GraphicPIDY = new List<int>();
            GraphicPIDZ = new List<int>();

            for (int i = 0; i < GraphicWidth; i++)
            {
                GraphicAccelX.Add(0);
                GraphicAccelY.Add(0);
                GraphicAccelZ.Add(0);

                GraphicGyroX.Add(0);
                GraphicGyroY.Add(0);
                GraphicGyroZ.Add(0);

                GraphicMagX.Add(0);
                GraphicMagY.Add(0);
                GraphicMagZ.Add(0);

                GraphicPIDX.Add(0);
                GraphicPIDY.Add(0);
                GraphicPIDZ.Add(0);
            }

            ClearCalibrationBuffer();

            ClearCalibrationValues();

            this.KeyPreview = true;

            Timer10.Start();

            //Timer20.Start();

            //Timer100.Start();

            //YawTrim.Value = YawCenter;

            /*YawTrimText.Text = YawCenter.ToString();
            PitchTrimText.Text = PitchCenter.ToString();
            RollTrimText.Text = RollCenter.ToString();

            RollP = RollPBar.Value;
            RollI = RollIBar.Value;
            RollD = RollDBar.Value;
            PitchP = PitchPBar.Value;
            PitchI = PitchIBar.Value;
            PitchD = PitchDBar.Value;
            YawP = YawPBar.Value;
            YawI = YawIBar.Value;
            YawD = YawDBar.Value;

            RollPText.Text = ((double)RollP / 128.0).ToString();
            RollIText.Text = ((double)RollI / 128.0).ToString();
            RollDText.Text = ((double)RollD / 128.0).ToString();
            PitchPText.Text = ((double)PitchP / 128.0).ToString();
            PitchIText.Text = ((double)PitchI / 128.0).ToString();
            PitchDText.Text = ((double)PitchD / 128.0).ToString();            
            YawPText.Text = ((double)YawP / 128.0).ToString();
            YawIText.Text = ((double)YawI / 128.0).ToString();
            YawDText.Text = ((double)YawD / 128.0).ToString();*/
        }

        private void ConnectionButton_Click(object sender, EventArgs e)
        {
            Connection = !Connection;

            if (Connection)
            {
                // Connection open
                serialPort1.Open();

                //serialPort1.Write(new byte[] {0xFF, 0xFF, 0xFF, 0xFF}, 0, 4);

                //int R = (int)((8000 - Roll) * RollRange + 4000 * (1 - RollRange)) + 8000;

                //int P = (int)((8000 - Pitch) * PitchRange + 4000 * (1 - PitchRange)) + 8000;

                //int T = (int)((8000 - Throttle.Value) * ThrottleRange + 4000 * (1 - ThrottleRange)) + 8000;

                //int Y = (int)((8000 - Yaw.Value) * YawRange + 4000 * (1 - YawRange)) + 8000;

                //serialPort1.Write(new byte[] { 0xFF, 0x02, (byte)(R >> 8), (byte)R }, 0, 4);

                //serialPort1.Write(new byte[] { 0xFF, 0x03, (byte)(P >> 8), (byte)P }, 0, 4);

                //serialPort1.Write(new byte[] { 0xFF, 0x04, (byte)(T >> 8), (byte)T }, 0, 4);

                //serialPort1.Write(new byte[] { 0xFF, 0x05, (byte)(Y >> 8), (byte)Y }, 0, 4);

                /*int Sens;

                if (GyroXHeadingHold.Checked)
                {
                    Sens = (int)(GyroXSensitivity.Maximum / 2 + 0.5 * GyroXSensitivity.Value * GyroXSensitivityRange + 0.25 * GyroXSensitivity.Maximum * (1 - GyroXSensitivityRange)) + 8000;
                }
                else
                {
                    Sens = (int)(GyroXSensitivity.Maximum / 2 - 0.5 * GyroXSensitivity.Value * GyroXSensitivityRange + 0.25 * GyroXSensitivity.Maximum * (1 - GyroXSensitivityRange)) + 8000;
                }
                serialPort1.Write(new byte[] { 0xFF, 0x06, (byte)(Sens >> 8), (byte)Sens }, 0, 4); 

                if (GyroYHeadingHold.Checked)
                {
                    Sens = (int)(GyroYSensitivity.Maximum / 2 + 0.5 * GyroYSensitivity.Value * GyroYSensitivityRange + 0.25 * GyroYSensitivity.Maximum * (1 - GyroYSensitivityRange)) + 8000;
                }
                else
                {
                    Sens = (int)(GyroYSensitivity.Maximum / 2 - 0.5 * GyroYSensitivity.Value * GyroYSensitivityRange + 0.25 * GyroYSensitivity.Maximum * (1 - GyroYSensitivityRange)) + 8000;
                }
                serialPort1.Write(new byte[] { 0xFF, 0x07, (byte)(Sens >> 8), (byte)Sens }, 0, 4);

                if (GyroZHeadingHold.Checked)
                {
                    Sens = (int)(GyroZSensitivity.Maximum / 2 + 0.5 * GyroZSensitivity.Value * GyroZSensitivityRange + 0.25 * GyroZSensitivity.Maximum * (1 - GyroZSensitivityRange)) + 8000;
                }
                else
                {
                    Sens = (int)(GyroZSensitivity.Maximum / 2 - 0.5 * GyroZSensitivity.Value * GyroZSensitivityRange + 0.25 * GyroZSensitivity.Maximum * (1 - GyroZSensitivityRange)) + 8000;
                }
                serialPort1.Write(new byte[] { 0xFF, 0x08, (byte)(Sens >> 8), (byte)Sens }, 0, 4);

                //serialPort1.Write(new byte[] { 0xFF, 0x13, (byte)Sensitivity, 0x00 }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x20, (byte)RollP, 0x00 }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x21, (byte)RollI, 0x00 }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x22, (byte)RollD, 0x00 }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x23, (byte)PitchP, 0x00 }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x24, (byte)PitchI, 0x00 }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x25, (byte)PitchD, 0x00 }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x26, (byte)YawP, 0x00 }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x27, (byte)YawI, 0x00 }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x28, (byte)YawD, 0x00 }, 0, 4);

                try
                {
                    int Shift = Int32.Parse(FrontMotorShift.Text) + 4000;

                    serialPort1.Write(new byte[] { 0xFF, 0x16, (byte)(Shift >> 8), (byte)Shift }, 0, 4);

                    Shift = Int32.Parse(LeftMotorShift.Text) + 4000;

                    serialPort1.Write(new byte[] { 0xFF, 0x17, (byte)(Shift >> 8), (byte)Shift }, 0, 4);

                    Shift = Int32.Parse(RightMotorShift.Text) + 4000;

                    serialPort1.Write(new byte[] { 0xFF, 0x18, (byte)(Shift >> 8), (byte)Shift }, 0, 4);

                    Shift = Int32.Parse(RearMotorShift.Text) + 4000;

                    serialPort1.Write(new byte[] { 0xFF, 0x19, (byte)(Shift >> 8), (byte)Shift }, 0, 4);
                }
                catch (Exception)
                { }*/

                ConnectionButton.Text = "Disconnect";
            }
            else
            {
                //serialPort1.Write(new byte[] { 0xFF, 0xFE, 0xFF, 0xFF }, 0, 4);

                serialPort1.Close();

                ConnectionButton.Text = "Connect";
            }
        }

        private void serialPort1_DataReceived(object sender, System.IO.Ports.SerialDataReceivedEventArgs e)
        {
            
        }

        private void SendButton_Click(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                string []Params = SendText.Text.Split(new char[] {' '});

                switch (Params[0])
                {
                    case "light":
                    {
                        if (Params[1] == "on")
                        {
                            serialPort1.Write(new byte[] { 0xFF, 0x01, 0x01, 0x00 }, 0, 4);

                            Console.Text += "light on\r\n";
                        }
                        else if (Params[1] == "off")
                        {
                            serialPort1.Write(new byte[] { 0xFF, 0x01, 0x00, 0x00 }, 0, 4);

                            Console.Text += "light off\r\n";
                        }

                        break;
                    }
                    case "MotorRange":
                    {
                        try
                        {
                            MotorRange = double.Parse(Params[1]);

                            Console.Text += "MotorRange set to " + Params[1] + "\r\n";
                        }
                        catch (Exception)
                        {
                            Console.Text += Params[1] + " is not a number\r\n";
                        }

                        break;
                    }
                    default:
                    {
                        if (SendText.Text.Length % 2 == 1)
                        {
                            SendText.Text += " ";
                        }

                        SendText.Text += "\r\n";

                        for (int i = 0; i < SendText.Text.Length; i += 2)
                        {
                            serialPort1.Write(new byte[] { 0xFF }, 0, 1);
                            //serialPort1.Write("$");

                            serialPort1.Write(SendText.Text[i].ToString() + SendText.Text[i + 1].ToString());
                        }
                        break;
                    }
                }

                SendText.Text = "";
            }*/
        }

        int kk = 0;

        private void Timer10_Tick(object sender, EventArgs e)
        {
            if (Connection)
            {
                while (serialPort1.BytesToRead >= 4)
                {
                    if (serialPort1.ReadByte() == 0xFF)
                    {
                        int Message = serialPort1.ReadByte();
                        int LParam =  serialPort1.ReadByte();
                        int HParam =  serialPort1.ReadByte();
                        int QParam =  serialPort1.ReadByte();
                        int RParam =  serialPort1.ReadByte();
                        byte[] P = new byte[] { (byte)LParam, (byte)HParam, (byte)QParam, (byte)RParam };
                        float Param = System.BitConverter.ToSingle(P, 0);
                        //float Param = (float)((RParam << 24) | (QParam << 16) | (HParam << 8) | (LParam));
                        //int Param = (((HParam << 8) | LParam) - 32768);

                       
                        //int Param = ((HParam << 8) | LParam) - 8000;

                        switch (Message)
                        {
                            case 1:
                            {
                                //GraphicAccelX.RemoveAt(0);

                                //GraphicAccelX.Add((int)(50 * Param /1) );

                                break;
                            }
                            case 2:
                            {
                                //GraphicAccelY.RemoveAt(0);

                                //GraphicAccelY.Add((int)(50 * Param / 1));

                                break;
                            }
                            case 3:
                            {
                                //GraphicAccelZ.RemoveAt(0);

                                //GraphicAccelZ.Add((int)(50 * Param / 1));

                                break;
                            }
                            case 4:
                            {
                                //GraphicGyroX.RemoveAt(0);

                                if (Param > -100 && Param < 100)
                                {
                                    //GraphicGyroX.Add((int)(20 * Param / 1));
                                }
                                else
                                {
                                    //GraphicGyroX.Add(0);
                                }
                                break;
                            }
                            case 5:
                            {
                                //GraphicGyroY.RemoveAt(0);

                                if (Param > -100 && Param < 100)
                                {
                                    //GraphicGyroY.Add((int)(20 * Param / 1));
                                }
                                else
                                {
                                    //GraphicGyroY.Add(0);
                                }
                                break;
                            }
                            case 6:
                            {
                                //GraphicGyroZ.RemoveAt(0);

                                if (Param > -100 && Param < 100)
                                {
                                    //GraphicGyroZ.Add((int)(20 * Param / 1));
                                }
                                else
                                {
                                    //GraphicGyroZ.Add(0);
                                }
                                break;
                            }
                            case 7:
                            {
                                //GraphicMagX.RemoveAt(0);

                                //GraphicMagX.Add((int)(20 * Param / 1));

                                if (Calibration)
                                {
                                    if (Param < minx)
                                        minx = Param;

                                    if (Param > maxx)
                                        maxx = Param;
                                }
                                ParamX = (int)Param;

                                break;
                            }
                            case 8:
                            {
                                //GraphicMagY.RemoveAt(0);

                                //GraphicMagY.Add((int)(20 * Param / 1));

                                if (Calibration)
                                {
                                    if (Param < miny)
                                        miny = Param;

                                    if (Param > maxy)
                                        maxy = Param;
                                }
                                ParamY = (int)Param;

                                break;
                            }
                            case 9:
                            {
                                //GraphicMagZ.RemoveAt(0);

                                //GraphicMagZ.Add((int)(20 * Param / 1));

                                if (Calibration)
                                {
                                    if (Param < minz)
                                        minz = Param;

                                    if (Param > maxz)
                                        maxz = Param;
                                }
                                ParamZ = (int)Param;

                                break;
                            }
                            case 10:
                            {
                                RollText.Text = (Math.Round(Param * 180 / Math.PI, 2)).ToString();

                                /*GraphicMagX.RemoveAt(0);

                                if (Param > -100 && Param < 100)
                                {
                                    GraphicMagX.Add((int)(30 * Param / 1));
                                }
                                else
                                {
                                    GraphicMagX.Add(0);
                                }*/

                                break;
                            }
                            case 11:
                            {
                                PitchText.Text = (Math.Round(Param * 180 / Math.PI, 2)).ToString();
                                
                                /*GraphicMagY.RemoveAt(0);
                                if (Param > -100 && Param < 100)
                                {
                                    GraphicMagY.Add((int)(30 * Param / 1));
                                }
                                else
                                {
                                    GraphicMagY.Add(0);
                                }*/
                                break;
                            }
                            case 12:
                            {
                                YawText.Text = (Math.Round(Param * 180 / Math.PI, 2)).ToString();
                                
                                /*GraphicMagZ.RemoveAt(0);
                                if (Param > -100 && Param < 100)
                                {
                                    GraphicMagZ.Add((int)(30 * Param / 1));
                                }
                                else
                                {
                                    GraphicMagZ.Add(0);
                                }*/
                                break;
                            }
                            case 13:
                            {
                                System.Diagnostics.Debug.WriteLine("1:" + Param);
                                break;
                            }
                            case 14:
                            {
                                System.Diagnostics.Debug.WriteLine("2:" + Param);
                                break;
                            }
                            case 15:
                            {
                                System.Diagnostics.Debug.WriteLine("3:" + Param);
                                break;
                            }
                            case 16:
                            {
                                System.Diagnostics.Debug.WriteLine("4:" + Param);
                                break;
                            }
                            case 17:
                            {
                                System.Diagnostics.Debug.WriteLine("5:" + Param);
                                break;
                            }
                            case 20:
                            {
                                GraphicGyroX.RemoveAt(0);

                                GraphicGyroX.Add((int)(30 * Param / 1));

                                break;
                            }
                            case 21:
                            {
                                GraphicGyroY.RemoveAt(0);

                                GraphicGyroY.Add((int)(30 * Param / 1));

                                break;
                            }
                            case 22:
                            {
                                GraphicGyroZ.RemoveAt(0);

                                GraphicGyroZ.Add((int)(30 * Param / 1));

                                break;
                            }
                            case 23:
                            {
                                GraphicAccelX.RemoveAt(0);

                                GraphicAccelX.Add((int)(30 * Param / 1));

                                break;
                            }
                            case 24:
                            {
                                GraphicAccelY.RemoveAt(0);

                                GraphicAccelY.Add((int)(30 * Param / 1));

                                break;
                            }
                            case 25:
                            {
                                GraphicAccelZ.RemoveAt(0);

                                GraphicAccelZ.Add((int)(30 * Param / 1));

                                break;
                            }
                            case 26:
                            {
                                GraphicPIDX.RemoveAt(0);

                                GraphicPIDX.Add((int)(30 * Param / 1));

                                break;
                            }
                            case 27:
                            {
                                GraphicPIDY.RemoveAt(0);

                                GraphicPIDY.Add((int)(30 * Param / 1));

                                break;
                            }
                            case 28:
                            {
                                GraphicPIDZ.RemoveAt(0);

                                GraphicPIDZ.Add((int)(30 * Param / 1));

                                break;
                            }
                                
                            /*case 0x04:
                            {
                                int v = (Param - 2000) * 2;

                                if (v > 8000) v = 8000;
                                if (v < 0) v = 0;

                                MotorFrontPower.Value = v;

                                break;
                            }
                            case 0x05:
                            {
                                int v = (Param - 2000) * 2;

                                if (v > 8000) v = 8000;
                                if (v < 0) v = 0;

                                MotorLeftPower.Value = v;

                                break;
                            }
                            case 0x06:
                            {
                                int v = (Param - 2000) * 2;

                                if (v > 8000) v = 8000;
                                if (v < 0) v = 0;

                                MotorRightPower.Value = v;

                                break;
                            }
                            case 0x07:
                            {
                                int v = (Param - 2000) * 2;

                                if (v > 8000) v = 8000;
                                if (v < 0) v = 0;

                                MotorRearPower.Value = v;

                                break;
                            }
                            case 0x08:
                            {
                                GraphicA.RemoveAt(0);

                                GraphicA.Add(Param - 4000);

                                break;
                            }
                            case 0x09:
                            {
                                GraphicB.RemoveAt(0);

                                GraphicB.Add(Param - 4000);

                                break;
                            }
                            case 0x10:
                            {
                                GraphicC.RemoveAt(0);

                                GraphicC.Add(Param - 4000);

                                break;
                            }*/
                            //default:
                            //{
                                //Console.Invoke(new MethodInvoker(delegate { Console.Text = /*((char)Message).ToString() +*/ Param.ToString() + " "; }));

                            //    break;
                            //}
                        }

                        /*if (Calibration && (Message == 7 || Message == 8 || Message == 9) && rnd.NextDouble() < 0.3)
                        {
                            Writer.WriteLine(ParamX + "\t" + ParamY + "\t" + ParamZ);

                            Writer.Flush();
                        }*/

                    }
                }
                RenderGraphics(AccelBuffer, AccelPanel1, GraphicAccelX, GraphicAccelY, GraphicAccelZ);
                RenderGraphics(GyroBuffer, GyroPanel1, GraphicGyroX, GraphicGyroY, GraphicGyroZ);
                RenderGraphics(MagBuffer, MagPanel1, GraphicMagX, GraphicMagY, GraphicMagZ);
                RenderGraphics(CompassBuffer, CompassPanel, GraphicPIDX, GraphicPIDY, GraphicPIDZ);

                if (Calibration)
                {
                    x1 = (maxx - minx) / 2;
                    x2 = (maxx + minx) / 2;

                    y1 = (maxy - miny) / 2;
                    y2 = (maxy + miny) / 2;

                    z1 = (maxz - minz) / 2;
                    z2 = (maxz + minz) / 2;
                }

                x1 = 1.0f; x2 = 0.0f;
                y1 = 1.0f; y2 = 0.0f;
                z1 = 1.0f; z2 = 0.0f;

                float x = (1.0f / x1) * (ParamX - x2);
                float y = (1.0f / y1) * (ParamY - y2);
                float z = (1.0f / z1) * (ParamZ - z2);

                float d = (float)Math.Sqrt(x * x + y * y + z * z);

                //RenderCompass(CompassBuffer, CompassPanel, Math.Atan2(y / d, x / d));

                if (Calibration)
                {
                    CompassLengthText.Text = Math.Round(d, 4).ToString();

                    CalibrationCompassX1Text.Text = x1.ToString();
                    CalibrationCompassX2Text.Text = x2.ToString();

                    CalibrationCompassY1Text.Text = y1.ToString();
                    CalibrationCompassY2Text.Text = y2.ToString();

                    CalibrationCompassZ1Text.Text = z1.ToString();
                    CalibrationCompassZ2Text.Text = z2.ToString();
                    /*if (GraphicMagX[GraphicWidth - 1] < minx)
                    {
                        minx = GraphicMagX[GraphicWidth - 1];
                    }
                    if (GraphicMagX[GraphicWidth - 1] > maxx)
                    {
                        maxx = GraphicMagX[GraphicWidth - 1];
                    }
                    if (GraphicMagY[GraphicWidth - 1] < miny)
                    {
                        miny = GraphicMagY[GraphicWidth - 1];
                    }
                    if (GraphicMagY[GraphicWidth - 1] > maxy)
                    {
                        maxy = GraphicMagY[GraphicWidth - 1];
                    }
                    if (GraphicMagZ[GraphicWidth - 1] < minz)
                    {
                        minz = GraphicMagZ[GraphicWidth - 1];
                    }
                    if (GraphicMagZ[GraphicWidth - 1] > maxz)
                    {
                        maxz = GraphicMagZ[GraphicWidth - 1];
                    }*/
                    RenderCalibration(MagXYBuffer, CompassPanelXY, GraphicMagX[GraphicWidth - 2], GraphicMagY[GraphicWidth - 2], GraphicMagX[GraphicWidth - 1], GraphicMagY[GraphicWidth - 1]);
                    RenderCalibration(MagXZBuffer, CompassPanelXZ, GraphicMagX[GraphicWidth - 2], GraphicMagZ[GraphicWidth - 2], GraphicMagX[GraphicWidth - 1], GraphicMagZ[GraphicWidth - 1]);
                    RenderCalibration(MagYZBuffer, CompassPanelYZ, GraphicMagZ[GraphicWidth - 2], GraphicMagY[GraphicWidth - 2], GraphicMagZ[GraphicWidth - 1], GraphicMagY[GraphicWidth - 1]);
                }
            }
        }

        private void RenderGraphics(BufferedGraphics buffer, Panel panel, List<int> GraphicX, List<int> GraphicY, List<int> GraphicZ)
        {
            Graphics g = buffer.Graphics;

            g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias;

            g.FillRectangle(new SolidBrush(Color.White), panel.DisplayRectangle);

            g.DrawLine(new Pen(Color.Gray, 1.0f), 0, 100, GraphicWidth, 100);

            try
            {
                for (int i = 0; i < GraphicWidth - 1; i++)
                {
                    g.DrawLine(new Pen(Color.FromArgb(128, 255, 0, 0), 1.0f), i, 100 - GraphicX[i], i + 1, 100 - GraphicX[i + 1]);
                }
            }
            catch (Exception)
            { }
            try
            {
                for (int i = 0; i < GraphicWidth - 1; i++)
                {
                    g.DrawLine(new Pen(Color.FromArgb(128, 0, 255, 0), 1.0f), i, 100 - GraphicY[i], i + 1, 100 - GraphicY[i + 1]);
                }
            }
            catch (Exception)
            { }
            try
            {
                for (int i = 0; i < GraphicWidth - 1; i++)
                {
                    g.DrawLine(new Pen(Color.FromArgb(128, 0, 0, 255), 1.0f), i, 100 - GraphicZ[i], i + 1, 100 - GraphicZ[i + 1]);
                }
            }
            catch (Exception)
            { } 

            buffer.Render();
            buffer.Render(panel.CreateGraphics());
        }

        private void RenderCompass(BufferedGraphics buffer, Panel panel, double Angle)
        {
            Graphics g = buffer.Graphics;

            g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias;

            g.FillRectangle(new SolidBrush(Color.White), panel.DisplayRectangle);

            //g.DrawLine(new Pen(Color.Gray, 1.0f), 0, 100, GraphicWidth, 100);

            g.DrawLine(new Pen(Color.Black), 100.0f, 100.0f, 100.0f + 80.0f * (float)Math.Cos(Angle + North), 100.0f - 80.0f * (float)Math.Sin(Angle + North));

            buffer.Render();
            buffer.Render(panel.CreateGraphics());
        }

        private void RenderCalibration(BufferedGraphics buffer, Panel panel, float pointX1, float pointY1, float pointX2, float pointY2)
        {
            Graphics g = buffer.Graphics;

            g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias;

            //g.FillRectangle(new SolidBrush(Color.White), panel.DisplayRectangle);

            //g.DrawLine(new Pen(Color.Gray, 1.0f), 0, 100, GraphicWidth, 100);

            g.DrawLine(new Pen(Color.Red, 1.0f), 100 - pointX1 * 0.5f, 100 - pointY1 * 0.5f, 100 - pointX2 * 0.5f, 100 - pointY2 * 0.5f); 

            buffer.Render();
            buffer.Render(panel.CreateGraphics());
        }

        private void GyroPanel1_Paint(object sender, PaintEventArgs e)
        {
            /*Graphics g = myBuffer.Graphics;// e.Graphics;

            g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias;

            g.FillRectangle(new SolidBrush(Color.Black), new Rectangle(0, 0, GyroPanel1.Width, GyroPanel1.Height));

            g.DrawLine(new Pen(Color.Gray, 1.0f), 0, 100, GraphicWidth, 100);

            for (int i = 0; i < GraphicWidth - 1; i++)
            {
                g.DrawLine(new Pen(Color.FromArgb(128, 255, 0, 0), 1.0f), i, 100 - GraphicX[i] * 0.025f, i + 1, 100 - GraphicX[i + 1] * 0.025f);
            }

            for (int i = 0; i < GraphicWidth - 1; i++)
            {
                g.DrawLine(new Pen(Color.FromArgb(128, 0, 255, 0), 1.0f), i, 100 - GraphicY[i] * 0.025f, i + 1, 100 - GraphicY[i + 1] * 0.025f);
            }

            for (int i = 0; i < GraphicWidth - 1; i++)
            {
                g.DrawLine(new Pen(Color.FromArgb(128, 0, 0, 255), 1.0f), i, 100 - GraphicZ[i] * 0.025f, i + 1, 100 - GraphicZ[i + 1] * 0.025f);
            }


            myBuffer.Render();*/

            //myBuffer.Render(GyroPanel1.CreateGraphics());
            /*for (int i = 0; i < GraphicWidth - 1; i++)
            {
                g.DrawLine(new Pen(Color.FromArgb(128, 0, 255, 255), 1.0f), i, 100 - GraphicA[i] * 0.025f, i + 1, 100 - GraphicA[i + 1] * 0.025f);
            }

            for (int i = 0; i < GraphicWidth - 1; i++)
            {
                g.DrawLine(new Pen(Color.FromArgb(128, 255, 0, 255), 1.0f), i, 100 - GraphicB[i] * 0.025f, i + 1, 100 - GraphicB[i + 1] * 0.025f);
            }

            for (int i = 0; i < GraphicWidth - 1; i++)
            {
                g.DrawLine(new Pen(Color.FromArgb(128, 255, 255, 0), 1.0f), i, 100 - GraphicC[i] * 0.025f, i + 1, 100 - GraphicC[i + 1] * 0.025f);
            }*/
        }

        private void ControlPanel_Paint(object sender, PaintEventArgs e)
        {
            /*Graphics g = e.Graphics;

            g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias;

            g.DrawLine(new Pen(Color.Gray, 1.0f), ((RollCenter - 3000) * ControlPanel.Width) / 2000 - 4, ControlPanel.Height - ((PitchCenter - 3000) * ControlPanel.Height) / 2000, ((RollCenter - 3000) * ControlPanel.Width) / 2000 + 4, ControlPanel.Height - ((PitchCenter - 3000) * ControlPanel.Height) / 2000);

            g.DrawLine(new Pen(Color.Gray, 1.0f), ((RollCenter - 3000) * ControlPanel.Width) / 2000, ControlPanel.Height - ((PitchCenter - 3000) * ControlPanel.Height) / 2000 - 4, ((RollCenter - 3000) * ControlPanel.Width) / 2000, ControlPanel.Height - ((PitchCenter - 3000) * ControlPanel.Height) / 2000 + 4);

            g.DrawEllipse(new Pen(Color.Black, 3.0f), ((Roll - 3000) * ControlPanel.Width) / 2000 - 1, ControlPanel.Height - ((Pitch - 3000) * ControlPanel.Height) / 2000 - 1, 3, 3);
        */}

        private void ControlPanel_MouseDown(object sender, MouseEventArgs e)
        {
            //MouseDown = true;
        }

        private void ControlPanel_MouseUp(object sender, MouseEventArgs e)
        {
            //MouseDown = false;
        }

        private void ControlPanel_MouseMove(object sender, MouseEventArgs e)
        {
            /*if (MouseDown)
            {
                Roll = (e.X * 255) / (ControlPanel.Width);
                Pitch = 255 - (e.Y * 255) / (ControlPanel.Height);

                if (Connection)
                {
                    serialPort1.Write(new byte[] { 0xFF, 0x02, (byte)(Roll * RollRange + 0.5 * 255 * (1 - RollRange)) }, 0, 3);

                    serialPort1.Write(new byte[] { 0xFF, 0x03, (byte)(Pitch * PitchRange + 0.5 * 255 * (1 - PitchRange)) }, 0, 3);
                }
                ControlPanel.Invalidate();
            }*/
        }

        private void Throttle_Scroll(object sender, EventArgs e)
        {
            if (Connection)
            {
                float T = (float)((Throttle.Value * ThrottleRange + 4000.0 * (1 - ThrottleRange)) / 8000.0);

                byte[] Bytes = BitConverter.GetBytes(T);
                serialPort1.Write(new byte[] { 0xFF, 20, Bytes[3], Bytes[2], Bytes[1], Bytes[0] }, 0, 6);
            }
        }

        private void Yaw_Scroll(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                int Y = (int)(Yaw.Value * YawRange + 4000 * (1 - YawRange)) + 8000;

                serialPort1.Write(new byte[] { 0xFF, 0x05, (byte)(Y >> 8), (byte)Y }, 0, 4);
            }*/
        }

        private void SkyBotForm_KeyDown(object sender, KeyEventArgs e)
        {
            /*if (e.KeyCode == Keys.W)
            {
                KeyDownPitchUp = true;
            }
            else if (e.KeyCode == Keys.S)
            {
                KeyDownPitchDown = true;
            }
            else if (e.KeyCode == Keys.D)
            {
                KeyDownRollUp = true;
            }
            else if (e.KeyCode == Keys.A)
            {
                KeyDownRollDown = true;
            }
            else if (e.KeyCode == Keys.E)
            {
                KeyDownYawUp = true;
            }
            else if (e.KeyCode == Keys.Q)
            {
                KeyDownYawDown = true;
            }
            else if (e.KeyCode == Keys.Z)
            {
                KeyDownThrottleUp = true;
            }
            else if (e.KeyCode == Keys.X)
            {
                KeyDownThrottleDown = true;
            }*/
        }

        private void SkyBotForm_KeyUp(object sender, KeyEventArgs e)
        {
            /*if (e.KeyCode == Keys.W)
            {
                KeyDownPitchUp = false;
            }
            else if (e.KeyCode == Keys.S)
            {
                KeyDownPitchDown = false;
            }
            else if (e.KeyCode == Keys.D)
            {
                KeyDownRollUp = false;
            }
            else if (e.KeyCode == Keys.A)
            {
                KeyDownRollDown = false;
            }
            else if (e.KeyCode == Keys.E)
            {
                KeyDownYawUp = false;
            }
            else if (e.KeyCode == Keys.Q)
            {
                KeyDownYawDown = false;
            }
            else if (e.KeyCode == Keys.Z)
            {
                KeyDownThrottleUp = false;
            }
            else if (e.KeyCode == Keys.X)
            {
                KeyDownThrottleDown = false;
            }*/
        }

        private void Timer20_Tick(object sender, EventArgs e)
        {
            /*int T = Throttle.Value;
            int Y = Yaw.Value;

            if (KeyDownPitchUp)
            {
                Forward = (Forward * 319.0 + 4000.0) / 320.0;
            }
            else
            {
                Forward *= 0.98;
            }
            if (KeyDownPitchDown)
            {
                Forward = (Forward * 319.0 - 4000.0) / 320.0;
            }
            else
            {
                Forward *= 0.98;
            }
            if (KeyDownRollUp)
            {
                Right = (Right * 319.0 + 4000.0) / 320.0;
            }
            else
            {
                Right *= 0.98;
            }
            if (KeyDownRollDown)
            {
                Right = (Right * 319.0 - 4000.0) / 320.0;
            }
            else
            {
                Right *= 0.98;
            }
            if (KeyDownYawUp)
            {
                Rotation = (Rotation * 319.0 + 4000.0) / 320.0;
            }
            else
            {
                Rotation *= 0.98;
            }
            if (KeyDownYawDown)
            {
                Rotation = (Rotation * 31.0 - 4000.0) / 32.0;
            }
            else
            {
                Rotation *= 0.98;
            }*/

            /*if (KeyDownPitchUp)
            {
                Pitch = Pitch + 3;
            }
            if (KeyDownPitchDown)
            {
                Pitch = Pitch - 3;
            }
            if (KeyDownRollUp)
            {
                Roll = Roll + 3;
            }
            if (KeyDownRollDown)
            {
                Roll = Roll - 3;
            }
            if (KeyDownYawUp)
            {
                Y = Y + 3;
            }
            if (KeyDownYawDown)
            {
                Y = Y - 3;
            }*/
            /*if (KeyDownThrottleUp)
            {
                T = T + 150;
            }
            if (KeyDownThrottleDown)
            {
                T = T - 150;
            }

            if (Pitch > PitchCenter) { Pitch -= 1; }
            if (Pitch < PitchCenter) { Pitch += 1; }

            if (Roll > RollCenter) { Roll -= 1; }
            if (Roll < RollCenter) { Roll += 1; }

            if (Y > YawCenter) { Y -= 1; }
            if (Y < YawCenter) { Y += 1; }

            Pitch = (Pitch > 5000) ? 5000 : Pitch;
            Pitch = (Pitch < 3000) ? 3000 : Pitch;

            Roll = (Roll > 5000) ? 5000 : Roll;
            Roll = (Roll < 3000) ? 3000 : Roll;

            Y = (Y > 5000) ? 5000 : Y;
            Y = (Y < 3000) ? 3000 : Y;

            T = (T > 8000) ? 8000 : T;
            T = (T < 0) ? 0 : T;

            Yaw.Value = Y;

            Throttle.Value = T;

            ControlPanel.Invalidate();*/
        }

        private void GyroXHeadingHold_CheckedChanged(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                int Sens;

                if (GyroXHeadingHold.Checked)
                {
                    Sens = (int)(GyroXSensitivity.Maximum / 2 + 0.5 * GyroXSensitivity.Value * GyroXSensitivityRange + 0.25 * GyroXSensitivity.Maximum * (1 - GyroXSensitivityRange)) + 8000;
                }
                else
                {
                    Sens = (int)(GyroXSensitivity.Maximum / 2 - 0.5 * GyroXSensitivity.Value * GyroXSensitivityRange + 0.25 * GyroXSensitivity.Maximum * (1 - GyroXSensitivityRange)) + 8000;
                }
                serialPort1.Write(new byte[] { 0xFF, 0x06, (byte)(Sens >> 8), (byte)Sens }, 0, 4); 
            }*/
        }

        private void GyroYHeadingHold_CheckedChanged(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                int Sens;

                if (GyroYHeadingHold.Checked)
                {
                    Sens = (int)(GyroYSensitivity.Maximum / 2 + 0.5 * GyroYSensitivity.Value * GyroYSensitivityRange + 0.25 * GyroYSensitivity.Maximum * (1 - GyroYSensitivityRange)) + 8000;
                }
                else
                {
                    Sens = (int)(GyroYSensitivity.Maximum / 2 - 0.5 * GyroYSensitivity.Value * GyroYSensitivityRange + 0.25 * GyroYSensitivity.Maximum * (1 - GyroYSensitivityRange)) + 8000;
                }
                serialPort1.Write(new byte[] { 0xFF, 0x07, (byte)(Sens >> 8), (byte)Sens }, 0, 4);

            }*/
        }

        private void GyroZHeadingHold_CheckedChanged(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                int Sens;

                if (GyroZHeadingHold.Checked)
                {
                    Sens = (int)(GyroZSensitivity.Maximum / 2 + 0.5 * GyroZSensitivity.Value * GyroZSensitivityRange + 0.25 * GyroZSensitivity.Maximum * (1 - GyroZSensitivityRange)) + 8000;
                }
                else
                {
                    Sens = (int)(GyroZSensitivity.Maximum / 2 - 0.5 * GyroZSensitivity.Value * GyroZSensitivityRange + 0.25 * GyroZSensitivity.Maximum * (1 - GyroZSensitivityRange)) + 8000;
                }
                serialPort1.Write(new byte[] { 0xFF, 0x08, (byte)(Sens >> 8), (byte)Sens }, 0, 4);
            }*/
        }

        private void GyroXSensitivity_Scroll(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                int Sens;

                if (GyroXHeadingHold.Checked)
                {
                    Sens = (int)(GyroXSensitivity.Maximum / 2 + 0.5 * GyroXSensitivity.Value * GyroXSensitivityRange + 0.25 * GyroXSensitivity.Maximum * (1 - GyroXSensitivityRange)) + 8000;
                }
                else
                {
                    Sens = (int)(GyroXSensitivity.Maximum / 2 - 0.5 * GyroXSensitivity.Value * GyroXSensitivityRange + 0.25 * GyroXSensitivity.Maximum * (1 - GyroXSensitivityRange)) + 8000;
                }
                serialPort1.Write(new byte[] { 0xFF, 0x06, (byte)(Sens >> 8), (byte)Sens }, 0, 4);
            }*/
        }

        private void GyroYSensitivity_Scroll(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                int Sens;

                if (GyroYHeadingHold.Checked)
                {
                    Sens = (int)(GyroYSensitivity.Maximum / 2 + 0.5 * GyroYSensitivity.Value * GyroYSensitivityRange + 0.25 * GyroYSensitivity.Maximum * (1 - GyroYSensitivityRange)) + 8000;
                }
                else
                {
                    Sens = (int)(GyroYSensitivity.Maximum / 2 - 0.5 * GyroYSensitivity.Value * GyroYSensitivityRange + 0.25 * GyroYSensitivity.Maximum * (1 - GyroYSensitivityRange)) + 8000;
                }
                serialPort1.Write(new byte[] { 0xFF, 0x07, (byte)(Sens >> 8), (byte)Sens }, 0, 4);

            }*/
        }

        private void GyroZSensitivity_Scroll(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                int Sens;

                if (GyroZHeadingHold.Checked)
                {
                    Sens = (int)(GyroZSensitivity.Maximum / 2 + 0.5 * GyroZSensitivity.Value * GyroZSensitivityRange + 0.25 * GyroZSensitivity.Maximum * (1 - GyroZSensitivityRange)) + 8000;
                }
                else
                {
                    Sens = (int)(GyroZSensitivity.Maximum / 2 - 0.5 * GyroZSensitivity.Value * GyroZSensitivityRange + 0.25 * GyroZSensitivity.Maximum * (1 - GyroZSensitivityRange)) + 8000;
                }
                serialPort1.Write(new byte[] { 0xFF, 0x08, (byte)(Sens >> 8), (byte)Sens }, 0, 4);
            }*/
        }

        private void ControlPanel_MouseClick(object sender, MouseEventArgs e)
        {
            /*if (e.Button == MouseButtons.Right)
            {
                RollCenter = (e.X * 8000) / ControlPanel.Width;

                PitchCenter = 8000 - (e.Y * 8000) / ControlPanel.Height;

                RollTrimText.Text = RollCenter.ToString();

                PitchTrimText.Text = PitchCenter.ToString();
            }*/
        }

        private void YawTrim_Scroll(object sender, EventArgs e)
        {
            //YawCenter = YawTrim.Value;

            //YawTrimText.Text = YawCenter.ToString();
        }

        private void Timer100_Tick(object sender, EventArgs e)
        {            
            /*if (Connection)
            {
                GyroPanel1.Invalidate();

                if ((byte)((int)Forward + 5000) == 255) Forward--;

                if ((byte)((int)Right + 5000) == 255) Right--;

                if ((byte)((int)Rotation + 5000) == 255) Rotation--;

                serialPort1.Write(new byte[] { 0xFF, 0x30, (byte)(((int)Forward + 5000) >> 8), (byte)((int)Forward + 5000) }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x31, (byte)(((int)Right + 5000) >> 8), (byte)((int)Right + 5000) }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x32, (byte)(((int)Rotation + 5000) >> 8), (byte)((int)Rotation + 5000) }, 0, 4);

                /*int R = (int)((8000 - Roll) * RollRange + 4000 * (1 - RollRange)) + 8000;

                int P = (int)((8000 - Pitch) * PitchRange + 4000 * (1 - PitchRange)) + 8000;

                int T = (int)(Throttle.Value * ThrottleRange + 4000 * (1 - ThrottleRange)) + 8000;

                int Y = (int)((8000 - Yaw.Value) * YawRange + 4000 * (1 - YawRange)) + 8000;

                serialPort1.Write(new byte[] { 0xFF, 0x02, (byte)(R >> 8), (byte)R }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x03, (byte)(P >> 8), (byte)P }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x04, (byte)(T >> 8), (byte)T }, 0, 4);

                serialPort1.Write(new byte[] { 0xFF, 0x05, (byte)(Y >> 8), (byte)Y }, 0, 4);*/
            //}*/
        }

        private void ButtonStart_Click(object sender, EventArgs e)
        {
            if (Connection)
            {
                if (Start)
                {
                    byte[] A = BitConverter.GetBytes(0.0f);
                    serialPort1.Write(new byte[] { 0xFF, 19, A[3], A[2], A[1], A[0] }, 0, 6);
                    
                    ButtonStart.Text = "START";

                    Start = false;
                }
                else
                {
                    byte[] A = BitConverter.GetBytes(1.0f);
                    serialPort1.Write(new byte[] { 0xFF, 19, A[3], A[2], A[1], A[0] }, 0, 6);

                    ButtonStart.Text = "BREAK";

                    Start = true;
                }
            }
        }

        private void PitchReverse_CheckedChanged(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                if (PitchReverse.Checked)
                {
                    serialPort1.Write(new byte[] { 0xFF, 0x10, 0x01, 0x00 }, 0, 4);
                }
                else
                {
                    serialPort1.Write(new byte[] { 0xFF, 0x10, 0x00, 0x00 }, 0, 4);
                }
            }*/
        }

        private void RollReverse_CheckedChanged(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                if (RollReverse.Checked)
                {
                    serialPort1.Write(new byte[] { 0xFF, 0x11, 0x01, 0x00 }, 0, 4);
                }
                else
                {
                    serialPort1.Write(new byte[] { 0xFF, 0x11, 0x00, 0x00 }, 0, 4);
                }
            }*/
        }

        private void YawReverse_CheckedChanged(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                if (YawReverse.Checked)
                {
                    serialPort1.Write(new byte[] { 0xFF, 0x12, 0x01, 0x00 }, 0, 4);
                }
                else
                {
                    serialPort1.Write(new byte[] { 0xFF, 0x12, 0x00, 0x00 }, 0, 4);
                }
            }*/
        }

        private void PitchTrimText_TextChanged(object sender, EventArgs e)
        {
            /*try
            {
                PitchCenter = Int32.Parse(PitchTrimText.Text);
            }
            catch (Exception) { }*/
        }

        private void RollTrimText_TextChanged(object sender, EventArgs e)
        {   
            /*try
            {
                RollCenter = Int32.Parse(RollTrimText.Text);
            }
            catch (Exception) { }*/
        }

        private void YawTrimText_TextChanged(object sender, EventArgs e)
        {   
            /*try
            {
                YawCenter = Int32.Parse(YawTrimText.Text);

                YawTrim.Value = YawCenter;
            }
            catch (Exception) { }*/
        }       

        private void ZeroPointButton_Click(object sender, EventArgs e)
        {
            if (Connection)
            {
                serialPort1.Write(new byte[] { 0xFF, 18, 0, 0, 0, 0 }, 0, 6);
            }
        }

        private void ResetGyrosButton_Click(object sender, EventArgs e)
        {
            if (Connection)
            {
                serialPort1.Write(new byte[] { 0xFF, 17, 0, 0, 0, 0 }, 0, 6);
            }
        }

        private void FrontMotorShift_TextChanged(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                int Shift = 0;

                try
                {
                    Shift = Int32.Parse(FrontMotorShift.Text) + 4000;

                    if ((byte)Shift == 255) Shift--;

                    serialPort1.Write(new byte[] { 0xFF, 0x16, (byte)(Shift >> 8), (byte)Shift }, 0, 4);
                }
                catch (Exception)
                { }
            }*/
        }

        private void LeftMotorShift_TextChanged(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                int Shift = 0;

                try
                {
                    Shift = Int32.Parse(LeftMotorShift.Text) + 4000;

                    if ((byte)Shift == 255) Shift--;

                    serialPort1.Write(new byte[] { 0xFF, 0x17, (byte)(Shift >> 8), (byte)Shift }, 0, 4);
                }
                catch (Exception)
                { }
            }*/
        }

        private void RightMotorShift_TextChanged(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                int Shift = 0;

                try
                {
                    Shift = Int32.Parse(RightMotorShift.Text) + 4000;

                    if ((byte)Shift == 255) Shift--;

                    serialPort1.Write(new byte[] { 0xFF, 0x18, (byte)(Shift >> 8), (byte)Shift }, 0, 4);
                }
                catch (Exception)
                { }
            }*/
        }

        private void RearMotorShift_TextChanged(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                int Shift = 0;

                try
                {
                    Shift = Int32.Parse(RearMotorShift.Text) + 4000;

                    if ((byte)Shift == 255) Shift--;

                    serialPort1.Write(new byte[] { 0xFF, 0x19, (byte)(Shift >> 8), (byte)Shift }, 0, 4);
                }
                catch (Exception)
                { }
            } */  
        }

        private void RollPBar_Scroll(object sender, EventArgs e)
        {
            /*RollP = RollPBar.Value;

            RollPText.Text = ((double)RollP / 128.0).ToString();

            if (Connection)
            {
                serialPort1.Write(new byte[] { 0xFF, 0x20, (byte)RollP, 0x00 }, 0, 4);
            }*/
        }

        private void RollIBar_Scroll(object sender, EventArgs e)
        {
            /*RollI = RollIBar.Value;

            RollIText.Text = ((double)RollI / 128.0).ToString();

            if (Connection)
            {
                serialPort1.Write(new byte[] { 0xFF, 0x21, (byte)RollI, 0x00 }, 0, 4);
            }*/
        }

        private void RollDBar_Scroll(object sender, EventArgs e)
        {
            /*RollD = RollDBar.Value;

            RollDText.Text = ((double)RollD / 128.0).ToString();

            if (Connection)
            {
                serialPort1.Write(new byte[] { 0xFF, 0x22, (byte)RollD, 0x00 }, 0, 4);
            }*/
        }

        private void PitchPBar_Scroll(object sender, EventArgs e)
        {
            /*PitchP = PitchPBar.Value;

            PitchPText.Text = ((double)PitchP / 128.0).ToString();

            if (Connection)
            {
                serialPort1.Write(new byte[] { 0xFF, 0x23, (byte)PitchP, 0x00 }, 0, 4);
            }*/
        }

        private void PitchIBar_Scroll(object sender, EventArgs e)
        {
            /*PitchI = PitchIBar.Value;

            PitchIText.Text = ((double)PitchI / 128.0).ToString();

            if (Connection)
            {
                serialPort1.Write(new byte[] { 0xFF, 0x24, (byte)PitchI, 0x00 }, 0, 4);
            }*/
        }

        private void PitchDBar_Scroll(object sender, EventArgs e)
        {
            /*PitchD = PitchDBar.Value;

            PitchDText.Text = ((double)PitchD / 128.0).ToString();

            if (Connection)
            {
                serialPort1.Write(new byte[] { 0xFF, 0x25, (byte)PitchD, 0x00 }, 0, 4);
            }*/
        }

        private void YawPBar_Scroll(object sender, EventArgs e)
        {
            /*YawP = YawPBar.Value;

            YawPText.Text = ((double)YawP / 128.0).ToString();

            if (Connection)
            {
                serialPort1.Write(new byte[] { 0xFF, 0x26, (byte)YawP, 0x00 }, 0, 4);
            }*/
        }

        private void YawIBar_Scroll(object sender, EventArgs e)
        {
            /*YawI = YawIBar.Value;

            YawIText.Text = ((double)YawI / 128.0).ToString();

            if (Connection)
            {
                serialPort1.Write(new byte[] { 0xFF, 0x27, (byte)YawI, 0x00 }, 0, 4);
            }*/
        }

        private void YawDBar_Scroll(object sender, EventArgs e)
        {
            /*YawD = YawDBar.Value;

            YawDText.Text = ((double)YawD / 128.0).ToString();

            if (Connection)
            {
                serialPort1.Write(new byte[] { 0xFF, 0x28, (byte)YawD, 0x00 }, 0, 4);
            }*/
        }

        private void ResetZeroButton_Click(object sender, EventArgs e)
        {
            /*if (Connection)
            {
                serialPort1.Write(new byte[] { 0xFF, 0x29, 0x00, 0x00 }, 0, 4);
            }*/
        }

        private void CalibrateCompassButton_Click(object sender, EventArgs e)
        {
            if (Calibration)
            {
                CalibrateCompassButton.Text = "Begin";

                Calibration = false;
            }
            else
            {
                CalibrateCompassButton.Text = "Stop";

                Calibration = true;
            }
        }

        private void ClearCalibrationCompassButton_Click(object sender, EventArgs e)
        {
            ClearCalibrationBuffer();

            ClearCalibrationValues();

            byte[] Zero = BitConverter.GetBytes(33000.0f);
            byte[] Scale = BitConverter.GetBytes(0.001f);
            serialPort1.Write(new byte[] { 0xFF, 10, Zero[3], Zero[2], Zero[1], Zero[0] }, 0, 6);
            serialPort1.Write(new byte[] { 0xFF, 11, Zero[3], Zero[2], Zero[1], Zero[0] }, 0, 6);
            serialPort1.Write(new byte[] { 0xFF, 12, Zero[3], Zero[2], Zero[1], Zero[0] }, 0, 6);
            serialPort1.Write(new byte[] { 0xFF, 13, Scale[3], Scale[2], Scale[1], Scale[0] }, 0, 6);
            serialPort1.Write(new byte[] { 0xFF, 14, Scale[3], Scale[2], Scale[1], Scale[0] }, 0, 6);
            serialPort1.Write(new byte[] { 0xFF, 15, Scale[3], Scale[2], Scale[1], Scale[0] }, 0, 6);
            serialPort1.Write(new byte[] { 0xFF, 16, 0, 0, 0, 0 }, 0, 6);
        }

        private void ClearCalibrationBuffer()
        {
            MagXYBuffer.Graphics.FillRectangle(new SolidBrush(Color.White), CompassPanelXY.DisplayRectangle);
            MagXYBuffer.Render();
            MagXYBuffer.Render(CompassPanelXY.CreateGraphics());

            MagXZBuffer.Graphics.FillRectangle(new SolidBrush(Color.White), CompassPanelXZ.DisplayRectangle);
            MagXZBuffer.Render();
            MagXZBuffer.Render(CompassPanelXZ.CreateGraphics());

            MagYZBuffer.Graphics.FillRectangle(new SolidBrush(Color.White), CompassPanelYZ.DisplayRectangle);
            MagYZBuffer.Render();
            MagYZBuffer.Render(CompassPanelYZ.CreateGraphics());
        }

        private void ClearCalibrationValues()
        {
            minx = Int32.MaxValue;
            maxx = Int32.MinValue;
            miny = Int32.MaxValue;
            maxy = Int32.MinValue;
            minz = Int32.MaxValue;
            maxz = Int32.MinValue;
        }

        private void CompassNorthText_TextChanged(object sender, EventArgs e)
        {
            try
            {
                North = double.Parse(CompassNorthText.Text);
            }
            catch (Exception)
            { }
        }

        private void button1_Click(object sender, EventArgs e)
        {
            serialPort1.Write(new byte[] { 0xFF, 0x10, 0x12, 0x34, 0xFF, 0x10, 0x01, 0x02 }, 0, 8);
            kk++;
            kk++;
                
                
        }

        private void AcceptCalibrationCompassButton_Click(object sender, EventArgs e)
        {
            CalibrateCompassButton.Text = "Begin";

            Calibration = false;
            byte[] A = BitConverter.GetBytes(x2);
            serialPort1.Write(new byte[] { 0xFF, 10, A[3], A[2], A[1], A[0] }, 0, 6);
            A = BitConverter.GetBytes(x1);
            serialPort1.Write(new byte[] { 0xFF, 13, A[3], A[2], A[1], A[0] }, 0, 6);
            A = BitConverter.GetBytes(y2);
            serialPort1.Write(new byte[] { 0xFF, 11, A[3], A[2], A[1], A[0] }, 0, 6);
            A = BitConverter.GetBytes(y1);
            serialPort1.Write(new byte[] { 0xFF, 14, A[3], A[2], A[1], A[0] }, 0, 6);
            A = BitConverter.GetBytes(z2);
            serialPort1.Write(new byte[] { 0xFF, 12, A[3], A[2], A[1], A[0] }, 0, 6);
            A = BitConverter.GetBytes(z1);
            serialPort1.Write(new byte[] { 0xFF, 15, A[3], A[2], A[1], A[0] }, 0, 6);

            serialPort1.Write(new byte[] { 0xFF, 16, 0, 0, 0, 0 }, 0, 6);

            ClearCalibrationValues();
        }

    }
}
